/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef TRANSPORT_UDP_H
#define TRANSPORT_UDP_H


//#include "ros/file_log.h"  //no need , use TRACE

//#include <ros/assert.h> //no need, use assert
#include <boost/bind.hpp>

#include <fcntl.h>
// For readv() and writev()
#include <sys/uio.h>



//#include <ros/types.h> //no need
#include "transport.hpp"
#include "../kcp/Kcp.hpp"

#include <pthread.h>
#include <thread>
#include <boost/thread/mutex.hpp>
#include <boost/thread/recursive_mutex.hpp>
#include <boost/thread/thread.hpp>

#include "../util/io.hpp"
#include "../util/util.hpp"
#include "../util/Trace.hpp"
#include "../util/poll_set.hpp"

#include <ctype.h>
#include <assert.h>

#include <mutex>
#include <memory>
#include <queue>
#include <utility>//pair
#include <vector>
#include <algorithm>
#include <condition_variable>

#include <pthread.h>
#include <unistd.h>
#include <stdlib.h>
#include <stdio.h>

//#include <ros/common.h> //no need

class TransportUDP;
typedef std::shared_ptr<TransportUDP> TransportUDPPtr;

class PollSet;

//the format of the rosudp heaader
// +-----------------------------+
// |        Connection ID        |
// +-----------------------------+
// |Opcode | Msg ID | Block #    |
// +-----------------------------+

//Opcode
#define ROS_UDP_DATA0 0  // This opcode is sent in the first datagram of a ROS message
#define ROS_UDP_DATAN 1  // All subsequent datagrams of a ROS message use this opcode
#define ROS_UDP_PING 2   // A heartbeat packet is sent periodically to let the other side know that the connection is still alive
#define ROS_UDP_ERR 3    // An error packet is used to signal that a connection is closing unnexpectedly
//Msg ID                 // This 8-bit value is incremented for each new message and is used to determine if datagrams have been dropped
//Block #                // This is 0 unless the opcode is DATA0, 
                         //       then this field contains the total number of UDPROS datagrams expected to complete the ROS message.
                         //       When the opcode is DATAN, this field contains the current datagram number.
/*
 * ROS messages are sent to the subscriber in one or more UDPROS datagrams. 
 * The first datagram contains the total number of datagrams to expect. 
 * Each datagram contains a header followed by serialized ROS message data. 
 * The maximum amount of message data per datagram is determined during the connection negotiation. 
 * Only the final datagram of a message will contain fewer than the maximum number of bytes.
 */

typedef struct TransportUDPHeader {
  uint32_t connection_id_;
  uint8_t op_;
  uint8_t message_id_;
  uint16_t block_;
} TransportUDPHeader;
                                                                                                                                             
/**
 * \brief UDPROS transport
 */
class TransportUDP : public Transport
{
public:


  enum Flags
  {
    SYNCHRONOUS = 1<<0,
  };

  TransportUDP(PollSet* poll_set, int flags = 0, int max_datagram_size = 0);
  virtual ~TransportUDP();

  /**
   * \brief Connect to a remote host ans listenning on localhost/localport
   * \param localhost The host to listen on
   * \param localport The port to listen on
   * \param host The hostname/IP to connect to
   * \param port The port to connect to
   * \return Whether or not the connection was successful
   */
  bool connect(const std::string& localhost, int localport, const std::string& remotehost, int remoteport, int connection_id, bool is_blocking=false);

  /**
   * \brief Returns the URI of the remote host
   */
  std::string getClientURI();

  /**
   * \brief Returns the port this transport is listening on
   */
  int getServerPort() const {return server_port_;}

  // overrides from Transport
  virtual int32_t read(uint8_t* buffer, uint32_t size);
  virtual int32_t write(uint8_t* buffer, uint32_t size);

  virtual void enableWrite();
  virtual void disableWrite();
  virtual void enableRead();
  virtual void disableRead();

  virtual void close();

  virtual std::string getTransportInfo();

  virtual bool requiresHeader() {return false;}

  virtual const char* getType() {return "KCPROS";}

  int getMaxDatagramSize() const {return max_datagram_size_;}

private:
  /**
   * \brief Initializes the assigned socket -- sets it to non-blocking and enables reading
   */
  bool initializeSocket(socket_fd_t &sock, bool set_blocking=false);


  void socketUpdate(int events);


  //socket_fd_t sock_;
  socket_fd_t sock_read_;
  socket_fd_t sock_write_;
  bool closed_;
  std::mutex close_mutex_;

  bool expecting_read_;
  bool expecting_write_;

  bool is_server_;
  sockaddr_in server_address_;
  sockaddr_in local_address_;
  int server_port_;
  int local_port_;

  std::string cached_remote_host_;

  PollSet* poll_set_;
  int flags_;

  uint32_t connection_id_;
  uint8_t current_message_id_;
  uint16_t total_blocks_;
  uint16_t last_block_;

  uint32_t max_datagram_size_;


  uint8_t* data_buffer_;
  uint8_t* data_start_;
  uint32_t data_filled_;

  uint8_t* reorder_buffer_;
  uint8_t* reorder_start_;
  TransportUDPHeader reorder_header_;
  uint32_t reorder_bytes_;


  //kcp
private:
  void thread_func_kcpupdate();
  void start_kcpupdate();
  void shutdown();
//private:
public:
  Kcp<true>::ptr kcp_layer_; 
  bool shutting_down_ = true;
  boost::thread thread_kcpupdate_;

private:
private:
  //as server recored the client ip & port
  //暂时用不上, 之前放在socketUpdate中用于recvfrom获取发送端地址
  std::string client_ip_;
  int client_port_;
  int flag_has_client_ = false;
private:
  //localport/host for bind; remoteport/ip for sendto and recvfrom
  sockaddr_in localaddr_;
  std::string localhost_;
  int localport_;
  sockaddr_in remoteaddr_;
  std::string remotehost_;
  int remoteport_;
  
  //make read wait until POLLIN happen.
  //静态方式初始化互斥锁和条件变量
  //pthread_mutex_t read_mutex_ = PTHREAD_MUTEX_INITIALIZER;
  //pthread_cond_t read_cond_ = PTHREAD_COND_INITIALIZER; 
  //bool has_recv=false;
  uint8_t* kcp_read_buffer_;
  //uint8_t* buff_ptr;
  //uint8_t* buff_ptr2;
  uint8_t* kcp_write_buffer_;

  std::thread thread_kcp_input_;
  //std::queue<uint8_t[]> udp_recv_queue_;
  //std::queue<H_Uint8> udp_recv_queue_;
  //std::queue<uint8_t*> udp_recv_queue_;
  std::queue<std::pair<uint8_t*, int>> udp_recv_queue_;
  //HQueue hqueue;
  std::mutex cv_mutex_;
  std::condition_variable cv_queue_;
};

void 
TransportUDP::thread_func_kcpupdate()
{
  while(!shutting_down_)
  {
    //TRACE("[INFO] kcp_layer_ start update");
    if(kcp_layer_->Update(iclock64()))
    {
      //kcp关闭时的回调函数可以设置在这里
      TRACE("[INFO] kcp_layer_ close");
    }
    //TRACE("[INFO] kcp_layer_ finish update");
    //usleep(1);
  }
}
void 
TransportUDP::start_kcpupdate()
{
  //只有上次的kcp层关闭了，才能重新打开这个线程
  if(shutting_down_)
  {
    shutting_down_ = false;

    //create server kcp_layer
    KcpOpt opt;
    opt.conv                = 666; 
    opt.interval            = 10;
    opt.snd_wnd_size        = 24;
    opt.rcv_wnd_size        = 20000048;
    opt.nodelay             = true;
    //opt.use_congestion      = true;
    opt.trigger_fast_resend = 10;
    opt.offline_standard    = 2000;
    opt.snd_queue_max_size  = 50000000;
    kcp_layer_              = std::make_shared<Kcp<true>>(opt);
    kcp_layer_->SetSendFunc([this](const void *buf, std::size_t size, void*)
                            {
                              ssize_t len = sendto(sock_write_, buf, size, 0,
                                                   (struct sockaddr*)&remoteaddr_, sizeof(remoteaddr_));
                              if(len == -1) //perror("sendto error");
                                TRACE("[ERROR] sendto error");
                            });
    //SetControl(new CongestionControl());

    
    thread_kcp_input_ = std::thread([this]{
                              while(1)
                              {
                                //if(hqueue.empty()){
                                  //continue;
                                //}

                                TRACE("[INFO] enter udp_recv_queue_'s pop while(1)");

                                //while(1)
                                //{

                                  int buf_len;
                                  uint8_t* buff_ptr2 = NULL;
                                {
                                  std::unique_lock<std::mutex> lk(cv_mutex_);
                                  cv_queue_.wait(lk, [this]{return !udp_recv_queue_.empty();});


                                  TRACE("get data from hqueue's front.");
                                  //buf_len = max_datagram_size_ + KcpAttr::KCP_HEADER_SIZE;
                                  buff_ptr2 = udp_recv_queue_.front().first;
                                  buf_len = udp_recv_queue_.front().second;
                                  udp_recv_queue_.pop();
                                  //memset(buff_ptr2, 0, buf_len);
                                  //memcpy(buff_ptr2, udp_recv_queue_.front(), buf_len);

                                  //for(int i = 0; i < buf_len; i++)
                                  //{
                                  //  buff_ptr2[i]=udp_recv_queue_.front()[i];
                                  //}
                                  TRACE("cp data from queue.front to h_uint8.");
                                  //H_Uint8 h_uint8(udp_recv_queue_.front().data, buf_len);
                                  //TRACE("queue.pop");
                                  //TRACE("cp data from h_uint8 to buff_ptr2");
                                  //memcpy(buff_ptr2, h_uint8.data, buf_len);


                                  //std::cout<<"[INFO] buf:"<<std::endl;
                                  //std::cout<<buff_ptr2<<std::endl;
#ifdef DEBUG_
                                  printf("[INFO] buf:%s\n", buff_ptr2);
                                  printf("[INFO] buf_len:%d\n", buf_len);
                                  printf("[INFO] udp_recv_queue_ size:%ld\n", udp_recv_queue_.size());
#endif
                                }
                                
//uint8_t tmp_uint8 = buff_ptr2[KcpAttr::KCP_HEADER_SIZE+sizeof(TransportUDPHeader)]; 
//printf("buff_ptr2[Headsize]char:%c\n", tmp_uint8); 
//printf("buff_ptr2[Headsize]int :%d\n", tmp_uint8); 
//printf("buff_ptr2[Headsize]uint8 0x:%0x\n", tmp_uint8); 
//printf("buff_ptr2[Headsize]maxASCII 0x:%0x\n",127); 
//printf("buff_ptr2[Headsize]isASCII:%d\n",isascii(tmp_uint8)); 

//char *src = static_cast<char*>((void*)buff_ptr2);
//KcpHdr *hdr = reinterpret_cast<KcpHdr*>(src);
//printf("hdr->cmd:%d\n", hdr->cmd);
//printf("PUSH_CMD:%d\n", KcpAttr::KCP_CMD_PUSH);

                                  int ret = kcp_layer_->Input(buff_ptr2, buf_len);
                                  delete [] buff_ptr2;
                                  buff_ptr2=NULL;
                                  
                                  if(ret < 0 ) 
                                  { 
                                    TRACE("[ERROR] kcp input error"); 
                                    assert(0);
                                    return ; 
                                  }
                                  else 
                                  { 
                                  }
     
                                     

                                  TRACE("[SOCKEET UPDATE]", "after input: recv_queue size", kcp_layer_->rcv_queue_.get().size());
                                  //TODO 判断数据头中，是kcp ack还是kcpdata，若是kcpdata，则调用read_cb_读取
                                  //如果收到的是rosudp header + raw data 的数据;
                                  //而不是kcp的ack、wask、ping、pong、wins等消息
                                  //则调用用户注册的读数据回调函数，读取数据, 否则不必处理，因为kcp input中已经处理过了
                                  if(buf_len > KcpAttr::KCP_HEADER_SIZE)
                                  {
                                    TRACE("[NUM RECV]", buf_len); 
                                    TRACE("[KCP_HEADER]", KcpAttr::KCP_HEADER_SIZE); 
                                    TRACE("[ROS_HEADER]", sizeof(TransportUDPHeader));
                                    TRACE("[READ_CB_]", read_cb_!=nullptr);
                                    TRACE("[EXPECTING READ]", expecting_read_);

                                    if (expecting_read_) 
                                    { 

                                      //TRACE2("[DEBUG DDDDDDD]:ROS_HEADER_SIZE:%d", sizeof(TransportUDPHeader));
                                      //TRACE2("[DEBUG DDDDDDD]:KCP_HEADER_SIZE:%d", KcpAttr::KCP_HEADER_SIZE );
                                      //TRACE2("[DEBUG DDDDDDD]:buf_len:%d", buf_len );

                                      if(read_cb_)
                                        read_cb_(shared_from_this()); 
                                    }
                                  }//end if(buf_len>KcpAttr::KCP_HEADER_SIZE)
                                //}//end while(!udp_recv_queue_.empty())
                                //usleep(1);
                              }//end while(1);
                            });//end thread create

    thread_kcpupdate_ = boost::thread(&TransportUDP::thread_func_kcpupdate, this);
  }
}
void 
TransportUDP::shutdown()
{
  if(shutting_down_)return ;
  shutting_down_ = true;
  if(thread_kcpupdate_.get_id() != boost::this_thread::get_id())
  {
    thread_kcpupdate_.join();
  }
  thread_kcp_input_.join();
}



TransportUDP::TransportUDP(PollSet* poll_set, int flags, int max_datagram_size)
: sock_read_(-1)
, sock_write_(-1)
, closed_(false)
, expecting_read_(false)
, expecting_write_(false)
, is_server_(false)
, server_address_{}
, local_address_{}
, server_port_(-1)
, local_port_(-1)
, poll_set_(poll_set)
, flags_(flags)
, connection_id_(0)
, current_message_id_(0)
, total_blocks_(0)
, last_block_(0)
, max_datagram_size_(max_datagram_size)
, data_filled_(0)
, reorder_buffer_(0)
, reorder_bytes_(0)
{
  // This may eventually be machine dependent
  if (max_datagram_size_ == 0)
    max_datagram_size_ = 1500;
  reorder_buffer_ = new uint8_t[max_datagram_size_];
  reorder_start_ = reorder_buffer_;
  data_buffer_ = new uint8_t[max_datagram_size_];
  data_start_ = data_buffer_;

  kcp_read_buffer_ = new uint8_t[max_datagram_size_ ];
  //buff_ptr = new uint8_t[max_datagram_size_ + KcpAttr::KCP_HEADER_SIZE];
  //buff_ptr2 = new uint8_t[max_datagram_size_ + KcpAttr::KCP_HEADER_SIZE];
  kcp_write_buffer_ = new uint8_t[max_datagram_size_ + KcpAttr::KCP_HEADER_SIZE];

  //kcp_layer的初始化，是在得到connection id之后再初始化
}

TransportUDP::~TransportUDP()
{
  shutdown();//shutdown kcp layer
  
  //pthread_mutex_destroy(&read_mutex_);
  //pthread_cond_destroy(&read_cond_);

  //poll_set_->delEvents(sock_read_, POLLIN);
  //poll_set_->delEvents(sock_write_, POLLOUT);
  
  //temporarily close without set disconnect callback function
  close();

  //ROS_ASSERT_MSG(sock_ == ROS_INVALID_SOCKET, "TransportUDP socket [%d] was never closed", sock_);
  if(sock_read_ != ROS_INVALID_SOCKET) { TRACE("[ASSERT MSG] TransportUDP read socket [", sock_read_, "] was never closed"); assert(0); }
  if(sock_write_ != ROS_INVALID_SOCKET) { TRACE("[ASSERT MSG] TransportUDP write socket [", sock_write_, "] was never closed"); assert(0); }
  delete [] reorder_buffer_;
  delete [] data_buffer_;
  
  delete [] kcp_read_buffer_;
  //delete [] buff_ptr;
  //delete [] buff_ptr2;
  delete [] kcp_write_buffer_ ;
}

void TransportUDP::socketUpdate(int events)
{
TRACE("[SOCKEET UPDATE]", "socket update !");
  {
    std::scoped_lock lock(close_mutex_);

    if (closed_)
    {
      return;
    }
  }

  if((events & POLLERR) ||
     (events & POLLHUP) ||
     (events & POLLNVAL))
  {
    TRACE("[LOG DEBUG] read Socket ", sock_read_, " closed with (ERR|HUP|NVAL) events ", events);
    TRACE("[LOG DEBUG] write Socket ", sock_write_, " closed with (ERR|HUP|NVAL) events ", events);
    close();
  }
  else
  {
    if ((events & POLLIN) /*&& expecting_read_ */)
    {
TRACE("[SOCKEET UPDATE]", "events: POLLIN !");
      //kcp_layer_->Flush();//send ack

      //kcp udp readv  before kcp input and recv
      ssize_t num_recv;
      struct sockaddr_in client_addr_;
      //char buffer[max_datagram_size_ + KcpAttr::KCP_HEADER_SIZE]={0};
      uint8_t* buff_ptr = new uint8_t[max_datagram_size_ + KcpAttr::KCP_HEADER_SIZE];
      memset(buff_ptr, 0, sizeof(buff_ptr));
      int buff_len = max_datagram_size_ + KcpAttr::KCP_HEADER_SIZE;
      int addr_len = sizeof(client_addr_);
      //num_recv = readv(sock_, iov, 1);
      num_recv = recvfrom(sock_read_, buff_ptr, buff_len, 0, 
                          (struct sockaddr*)&client_addr_, (socklen_t*)&addr_len);

      if(num_recv<0)
      {
        //delete [] buff_ptr;
        if(last_socket_error_is_would_block()) 
        { 
          //num_recv = 0; break; //出错了 
        }
        else 
        { 
          TRACE("[LOG DEBUG] readv() failed with error [", last_socket_error_string(), "]"); 
          //close(); //break;//出错了 //暂且return
          return ; 
        }
      }
      else if(num_recv==0) 
      { 
        TRACE("LOG DEBUG] read Socket [", sock_read_,"] received 0/", max_datagram_size_+KcpAttr::KCP_HEADER_SIZE, " bytes, closing"); 
        //delete [] buff_ptr;
        //close(); 
        return ; 
      }
      else if(num_recv<KcpAttr::KCP_HEADER_SIZE) 
      { 
        TRACE("[ERROR] read Socket [", sock_read_, "] received short header [", int(num_recv), " bytes]:", last_socket_error_string()); 
        //delete [] buff_ptr;
        //close(); 
        return ;
      }

      TRACE("[INFO] udp_recv_queue_.push()"); 

      //这里装填时从右到左的顺序，所以卸货时也从右往左就好。
      //std::vector<uint8_t> vtmp(num_recv, 0);
      //for(int i = 0; i < num_recv; i++)
      //{
      //  vtmp[i] = buff_ptr[i];
      //}

      //reverse(vtmp.begin(), vtmp.end());
      //udp_recv_queue_.push(vtmp);
      std::unique_lock<std::mutex> lk(cv_mutex_);

      //hqueue.push(buff_ptr, num_recv);
      //udp_recv_queue_.push(H_Uint8(buff_ptr, num_recv));
      udp_recv_queue_.push(std::pair<uint8_t*, int>(buff_ptr, num_recv));
#ifdef DEBUG_
      printf("[INFO] num_recv:%ld\n", num_recv);
      //printf("[INFO] size of buff_ptr:%ld", strlen(buff_ptr));
      printf("[INFO] front.size(): %ld\n",  udp_recv_queue_.size());
#endif
      //delete [] buff_ptr;

      cv_queue_.notify_all();
/*
*/
//      int ret = kcp_layer_->Input(buff_ptr, num_recv);
//      
//      if(ret < 0 ) 
//      { 
//        TRACE("[ERROR] kcp input error"); 
//        assert(0);
//        return ; 
//      }
//      else 
//      { 
//      }
//     
//
//      TRACE("[SOCKEET UPDATE]", "after input: recv_queue size", kcp_layer_->rcv_queue_.get().size());
//      //TODO 判断数据头中，是kcp ack还是kcpdata，若是kcpdata，则调用read_cb_读取
//      //如果收到的是rosudp header + raw data 的数据;
//      //而不是kcp的ack、wask、ping、pong、wins等消息
//      //则调用用户注册的读数据回调函数，读取数据, 否则不必处理，因为kcp input中已经处理过了
//      if(num_recv > KcpAttr::KCP_HEADER_SIZE)
//      {
//        TRACE("[NUM RECV]", buf_len); 
//        TRACE("[KCP_HEADER]", KcpAttr::KCP_HEADER_SIZE); 
//        TRACE("[ROS_HEADER]", sizeof(TransportUDPHeader));
//        TRACE("[READ_CB_]", read_cb_!=nullptr);
//        TRACE("[EXPECTING READ]", expecting_read_);
//
//        if (expecting_read_) 
//        { 
//
//          TRACE("[DEBUG DDDDDDD]" );
//
//          if(read_cb_)
//            read_cb_(shared_from_this()); 
//        }
//      }//end if(buf_len>KcpAttr::KCP_HEADER_SIZE)
    }

    if ((events & POLLOUT) /*&& expecting_write_*/)
    {
      if (write_cb_ && expecting_write_)
      {
        write_cb_(shared_from_this());
      }
    }
  }
}

std::string TransportUDP::getTransportInfo()
{
  std::stringstream str;
  str << "UDPROS connection on port " << local_port_ << " to [" << cached_remote_host_ << "]";
  return str.str();
}

//ros本身如果host是无效ip的话，会自动制定一个有效ip替代
//但是我需要把它简化一下，因此默认host必须有效，如果无效的话，就报错就行了
bool TransportUDP::connect(const std::string& localhost, int localport, const std::string& remotehost, int remoteport, int connection_id, bool is_blocking)
{
  is_server_ = true;//信道双端平等，因此双方暂且都设置为 is_server_=true; 
  
  localhost_ = localhost;
  localport_ = localport;

  remotehost_ = remotehost;
  remoteport_ = remoteport;

  /*--------------------------sock_read connect to remote ip/port------------------------------*/
  if (!isHostAllowed(remotehost_))
    return false; // adios amigo
  //create sock_ and set connection_id_
  sock_write_ = socket(AF_INET, SOCK_DGRAM, 0);
  connection_id_ = connection_id;

  if (sock_write_ == ROS_INVALID_SOCKET) { TRACE("[ERROR] socket() failed with error [",  last_socket_error_string(), "]"); return false; }


  // if this remote host is a invalid address, set a valid IP address instead of.
  if (inet_addr(remotehost_.c_str()) == INADDR_NONE) { close(); TRACE("[ERROR] couldn't resolve remotehost_ [", remotehost_.c_str(), "]"); return false; }

  //set remote ip and port
  //sockaddr_in sin = {};
  remoteaddr_.sin_family = AF_INET;
  remoteaddr_.sin_addr.s_addr = inet_addr(remotehost_.c_str()); // already an IP addr
  remoteaddr_.sin_port = htons(remoteport_);

  //connect 
  if (::connect(sock_write_, (sockaddr *)&remoteaddr_, sizeof(remoteaddr_))) 
  { TRACE("[LOG DEBUG] Connect to udpros remotehost_ [", remotehost_.c_str(), ":", remoteport_, "] failed with error [",  last_socket_error_string(), "]"); close(); return false; }

  // from daniel stonier:
  std::stringstream ss;
  ss << remotehost_ << ":" << remoteport_ << " on socket " << sock_write_;
  cached_remote_host_ = ss.str();

  if (!initializeSocket(sock_write_, is_blocking)) { return false; }
  TRACE("[LOG DEBUG] Connect succeeded to [", remotehost_.c_str(), ":", remoteport_, "] on socket [", sock_write_, "]");

  /*--------------------------sock_write bind with local ip/port------------------------------*/
  sock_read_ = socket(AF_INET, SOCK_DGRAM, 0);
  if(sock_read_ == ROS_INVALID_SOCKET){TRACE("[ERROR] sock_read_ create failed with error [", last_socket_error_string(), "]");return false;}

  server_address_.sin_family  = AF_INET; server_address_.sin_port    = htons(localport_); server_address_.sin_addr.s_addr = inet_addr(localhost_.c_str()); 
  local_address_.sin_family  = AF_INET; local_address_.sin_port    = htons(localport_); local_address_.sin_addr.s_addr = inet_addr(localhost_.c_str()); 

  if(bind(sock_read_, (sockaddr *)&local_address_, sizeof(local_address_)) < 0){TRACE("[ERROR] bind() failed with error [", last_socket_error_string(), "]");return false;}

  socklen_t len = sizeof(server_address_);
  getsockname(sock_read_, (sockaddr*)&server_address_, &len);
  server_port_ = ntohs(server_address_.sin_port);
  TRACE("[LOG DEBUG] listenning on port [", server_port_, "]");
  if(!initializeSocket(sock_read_), is_blocking){return false;}
  enableRead();
  poll_set_->addEvents(sock_read_, POLLIN);
  poll_set_->addEvents(sock_write_, POLLIN);


  /*--------------------------start kcp update------------------------------*/
  start_kcpupdate();//start kcp update thread

  //TRACE("[EXPECTING READ]", expecting_read_);

  return true;
}

bool TransportUDP::initializeSocket(socket_fd_t &sock, bool set_blocking)
{
  assert(sock != ROS_INVALID_SOCKET);


  //if (!(flags_ & SYNCHRONOUS))
  if(set_blocking==false)
  {
	  int result = set_non_blocking(sock);
	  if ( result != 0 ) {
	      TRACE("[ERROR] setting socket [", sock, "] as non_blocking failed with error [", result, "]");
      close();
      return false;
    }
  }

  socklen_t len = sizeof(local_address_);
  getsockname(sock, (sockaddr *)&local_address_, &len);
  local_port_ = ntohs(local_address_.sin_port);

  assert(poll_set_ || (flags_ & SYNCHRONOUS));
  if (poll_set_)
  {
    poll_set_->addSocket(sock, std::bind(&TransportUDP::socketUpdate, this, std::placeholders::_1), shared_from_this());
  }

  return true;
}

void TransportUDP::close()
{
  Callback disconnect_cb;

  if (!closed_)
  {
    {
      std::scoped_lock lock(close_mutex_);

      if (!closed_)
      {
        closed_ = true;

        TRACE("[LOG DEBUG] READ socket [", sock_read_, "] closed");
        TRACE("[LOG DEBUG] WRITE socket [", sock_write_, "] closed");

        assert(sock_read_ != ROS_INVALID_SOCKET);
        assert(sock_write_ != ROS_INVALID_SOCKET);

        if (poll_set_)
        {
          poll_set_->delSocket(sock_read_);
          poll_set_->delSocket(sock_write_);
        }

        if ( close_socket(sock_read_) != 0 ) { TRACE("[ERROR] Error closing read socket [", sock_read_, "]: [", last_socket_error_string(), "]"); }
        if ( close_socket(sock_write_) != 0 ) { TRACE("[ERROR] Error closing write socket [", sock_write_, "]: [", last_socket_error_string(), "]"); }

        sock_read_ = ROS_INVALID_SOCKET;
        sock_write_ = ROS_INVALID_SOCKET;

        disconnect_cb = disconnect_cb_;

        disconnect_cb_ = Callback();
        read_cb_ = Callback();
        write_cb_ = Callback();
      }
    }
  }

  if (disconnect_cb)
  {
    disconnect_cb(shared_from_this());
  }
}

int32_t TransportUDP::read(uint8_t* buffer, uint32_t size)
{
  //TRACE2("[READ DEBUG]", "recv_queue size", kcp_layer_->rcv_queue_.get().size());
  if(kcp_layer_->rcv_queue_.get().size()<=0)
  {
    //TRACE("[READ DEBUG]", "recv_queue size <=0 ");
    return -1;//表示kcp内核接收队列为空，没发读出数据
  }
  TRACE("[READ DEBUG]", "recv_queue size", kcp_layer_->rcv_queue_.get().size());
  

  TRACE("[READ DEBUG]", "read start");
  {
    std::scoped_lock lock(close_mutex_);
    if (closed_)
    {
      TRACE("[LOG DEBUG] Tried to read on a closed socket [", sock_read_, "]");
      return -1;
    }
  }

  if(shutting_down_)//check if the kcp update thread is start
  {
    TRACE("[ERROR] You should call connect or createIncoming first!");
    assert(0);
  }

  assert((int32_t)size > 0);

  uint32_t bytes_read = 0;

  TRACE("[READ DEBUG]", "before while");

  while (bytes_read < size)  // 循环读取，直到读够size大小的buffer为止
  {

  TRACE("[READ DEBUG]while");

    TransportUDPHeader header;

    // Get the data either from the reorder buffer or the socket
    // copy_bytes will contain the read size.
    // from_previous is true if the data belongs to the previous UDP datagram.
    uint32_t copy_bytes = 0;
    bool from_previous = false;
    if (reorder_bytes_)
    {
  TRACE("[READ DEBUG]", "if reorder_bytes_");
      if (reorder_start_ != reorder_buffer_)
      {
  TRACE("[READ DEBUG]", "if reorder_start_!=reorder_buffer_");
        from_previous = true;
      }

      copy_bytes = std::min(size - bytes_read, reorder_bytes_);
      header = reorder_header_;
      memcpy(buffer + bytes_read, reorder_start_, copy_bytes);
      reorder_bytes_ -= copy_bytes;
      reorder_start_ += copy_bytes;
    }
    else
    {
      if (data_filled_ == 0)
      {
  TRACE("[READ DEBUG]", "if data_filled==0");

  
  TRACE("[READ DEBUG]", "recv_queue size", kcp_layer_->rcv_queue_.get().size());

        //uint8_t* kcp_read_buffer_;
        //kcp_read_buffer_ = new uint8_t[max_datagram_size_ + KcpAttr::KCP_HEADER_SIZE];
        memset(kcp_read_buffer_, 0, sizeof(kcp_read_buffer_));
        ssize_t num_bytes = kcp_layer_->Recv(kcp_read_buffer_, max_datagram_size_);

        TRACE("[RECV NUM BYTES]:", num_bytes);

        if(num_bytes<0)
        {
          TRACE("[LOG DEBUG] the size of buf greater data size!");
          //close();
          break;
        }
        else if(num_bytes==0)
        {
          TRACE("[LOG DEBUG] received 0 bytes.");
          //close();
          return -1;
        }
        else if(num_bytes < (unsigned) sizeof(header))
        {
          TRACE("[ERROR] received short header");
          //close();
          return -1;
        }

        //kcp buffer -> header && data_buffer_
        memcpy(&header, kcp_read_buffer_, sizeof(header));
        memcpy(data_buffer_, kcp_read_buffer_ + sizeof(header), num_bytes - sizeof(header));

        //delete [] kcp_read_buffer_;

	  	  num_bytes -= sizeof(header);
        data_filled_ = num_bytes;
        data_start_ = data_buffer_;
      }
      else
      {
  TRACE("[READ DEBUG]", "if data_filled!=0");
        from_previous = true;
      }

      copy_bytes = std::min(size - bytes_read, data_filled_);
      // Copy from the data buffer, whether it has data left in it from a previous datagram or
      // was just filled by readv()
      memcpy(buffer + bytes_read, data_start_, copy_bytes);
      data_filled_ -= copy_bytes;
      data_start_ += copy_bytes;
    }


    if (from_previous)
    {
  TRACE("[READ DEBUG]", "if from_previous");
      // We are simply reading data from the last UDP datagram, nothing to
      // parse
      bytes_read += copy_bytes;
    }
    else
    {
  TRACE("[READ DEBUG]", "if not from_previous");
      // This datagram is new, process header
      switch (header.op_)
      {
        case ROS_UDP_DATA0:
  TRACE("[READ DEBUG]", "switch header.op: DATA0");
          if (current_message_id_)
          {
  TRACE("[READ DEBUG]", "if current_message_id_");
            TRACE("[DEBUG] Received new message [", header.message_id_, ":", header.block_, "],", 
                  " while still working on [", current_message_id_, "] (block ", last_block_ + 1, " of ", total_blocks_, ")");
            reorder_header_ = header;

            // Copy the entire data buffer to the reorder buffer, as we will
            // need to replay this UDP datagram in the next call.
            reorder_bytes_ = data_filled_ + (data_start_ - data_buffer_);
            memcpy(reorder_buffer_, data_buffer_, reorder_bytes_);
            reorder_start_ = reorder_buffer_;
            current_message_id_ = 0;
            total_blocks_ = 0;
            last_block_ = 0;

            data_filled_ = 0;
            data_start_ = data_buffer_;
            return -1;
          }
          total_blocks_ = header.block_;
          last_block_ = 0;
          current_message_id_ = header.message_id_;
          break;
        case ROS_UDP_DATAN:
  TRACE("[READ DEBUG]", "switch header.op: DATAN");
          if (header.message_id_ != current_message_id_)
          {
            TRACE("[DEBUG] Message Id mismatch: ", header.message_id_, " != ", current_message_id_);
            data_filled_ = 0; // discard datagram
            return 0;
          }
          if (header.block_ != last_block_ + 1)
          {
            TRACE("[DEBUG] Expected block ", last_block_ + 1, ", received ", header.block_);
            data_filled_ = 0; // discard datagram
            return 0;
          }
          last_block_ = header.block_;

          break;
        default:
  TRACE("[READ DEBUG]", "switch header.op: default");
          TRACE("[ERROR] Unexpected UDP header OP [", header.op_, "]");
          return -1;
      }

      bytes_read += copy_bytes;

      if (last_block_ == (total_blocks_ - 1))
      {
  TRACE("[READ DEBUG]", "if last_block == total_blocks -1" );
        current_message_id_ = 0;
        break;
      }
    }
  }
  TRACE("[READ DEBUG]", "read end");
  return bytes_read;
}

int32_t TransportUDP::write(uint8_t* buffer, uint32_t size)
{
  {
    std::scoped_lock lock(close_mutex_);

    if (closed_)
    {
      TRACE("[LOG DEBUG] Tried to write on a closed socket [", sock_write_, "]");
      return -1;
    }
  }

  if(shutting_down_)//check if the kcp update thread is start
  {
    TRACE("[ERROR] You should call connect or createIncoming first!");
    assert(0);
  }

  TRACE("size:", size);
  assert((int32_t)size > 0);

  const uint32_t max_payload_size = max_datagram_size_ - sizeof(TransportUDPHeader);

  uint32_t bytes_sent = 0;
  uint32_t this_block = 0;
  if (++current_message_id_ == 0)
    ++current_message_id_;
  while (bytes_sent < size)
  {
    TransportUDPHeader header;
    header.connection_id_ = connection_id_;
    header.message_id_ = current_message_id_;
    if (this_block == 0)
    {
      header.op_ = ROS_UDP_DATA0;
      header.block_ = (size + max_payload_size - 1) / max_payload_size;
    }
    else
    {
      header.op_ = ROS_UDP_DATAN;
      header.block_ = this_block;
    }
    ++this_block;

    // ros udp writev
    /*
    struct iovec iov[2];
    iov[0].iov_base = &header;
    iov[0].iov_len = sizeof(header);
    iov[1].iov_base = buffer + bytes_sent;
    iov[1].iov_len = std::min(max_payload_size, size - bytes_sent);

    ssize_t num_bytes = writev(sock_, iov, 2);
    */

    // kcp writev
    //uint8_t* kcp_write_buffer_;
    //kcp_write_buffer_ = new uint8_t[max_datagram_size_ + KcpAttr::KCP_HEADER_SIZE];
    memset(kcp_write_buffer_, 0, sizeof(kcp_write_buffer_));
    memcpy(kcp_write_buffer_, &header, sizeof(header));
    memcpy(kcp_write_buffer_ + sizeof(header) , buffer + bytes_sent, std::min(max_payload_size, size - bytes_sent));
    ssize_t num_bytes = kcp_layer_->Send( kcp_write_buffer_, sizeof(header)+std::min(max_payload_size, size - bytes_sent));
    //delete [] kcp_write_buffer_;
    //kcp_layer_->Flush();//暂且直接发


    //when = 0, means send to the kcp_snd_queue successfully.
    //else = -1, means failed.
    if(num_bytes==0)num_bytes=sizeof(header)+std::min(max_payload_size, size - bytes_sent);

    //usleep(100);
    if (num_bytes < 0)
    {
      if( !last_socket_error_is_would_block() ) // Actually EAGAIN or EWOULDBLOCK on posix
      {
        TRACE("[LOG DEBUG] writev() failed with error [", last_socket_error_string(), "]");
        //close();
        break;
      }
      else
      {
        num_bytes = 0;
        --this_block;
      }
    }
    else if (num_bytes < (unsigned) sizeof(header))
    {
      TRACE("[LOG DEBUG] Socket [", sock_write_, "] short write (", int(num_bytes), " bytes), closing");
      //close();
      break;
    }
    else
    {
      num_bytes -= sizeof(header);
    }
    bytes_sent += num_bytes;
  }

  return bytes_sent;
}

void TransportUDP::enableRead()
{
  {
    std::scoped_lock lock(close_mutex_);
  
    if (closed_)
    {
      return;
    }
  }

  if (!expecting_read_)
  {
    //poll_set_->addEvents(sock_read_, POLLIN);
    expecting_read_ = true;
  }
}

void TransportUDP::disableRead()
{
  assert(!(flags_ & SYNCHRONOUS));

  {
    std::scoped_lock lock(close_mutex_);

    if (closed_)
    {
      return;
    }
  }

  if (expecting_read_)
  {
    //poll_set_->delEvents(sock_read_, POLLIN);
    expecting_read_ = false;
  }
}

void TransportUDP::enableWrite()
{
  {
    std::scoped_lock lock(close_mutex_);

    if (closed_)
    {
      return;
    }
  }

  if (!expecting_write_)
  {
    //poll_set_->addEvents(sock_write_, POLLOUT);
    expecting_write_ = true;
  }
}

void TransportUDP::disableWrite()
{
  {
    std::scoped_lock lock(close_mutex_);

    if (closed_)
    {
      return;
    }
  }

  if (expecting_write_)
  {
    //poll_set_->delEvents(sock_write_, POLLOUT);
    expecting_write_ = false;
  }
}


std::string TransportUDP::getClientURI()
{
  assert(!is_server_);

  sockaddr_storage sas;
  socklen_t sas_len = sizeof(sas);
  getpeername(sock_write_, (sockaddr *)&sas, &sas_len);

  sockaddr_in *sin = (sockaddr_in *)&sas;

  char namebuf[128] = {};
  int port = ntohs(sin->sin_port);
  strncpy(namebuf, inet_ntoa(sin->sin_addr), sizeof(namebuf)-1);

  std::string ip = namebuf;
  std::stringstream uri;
  uri << ip << ":" << port;

  return uri.str();
}

#endif
